package org._649mset;

import edu.wpi.first.wpilibj.*;

public class ArcadeDT {

    final int VICTOR_COUNT = 4;
    double[] pseudoDT;
    Victor[] DT;

    public ArcadeDT() {
        pseudoDT = new double[VICTOR_COUNT];
        DT = new Victor[VICTOR_COUNT];
        for(int i = 0; i < VICTOR_COUNT; i++) {
            pseudoDT[i] = 0;
            DT[i] = new Victor(i+1);
        }
    }

    public ArcadeDT(int[] pwm) {
        pseudoDT = new double[VICTOR_COUNT];
        DT = new Victor[VICTOR_COUNT];
        for(int i = 0; i < VICTOR_COUNT; i++) {
            pseudoDT[i] = 0;
            DT[i] = new Victor(pwm[i]);
        }
    }

    public double[] getPseudoDT() {
        return pseudoDT;
    }

    public Victor[] getDT() {
        return DT;
    }

    public void drive(double spd, double rot) {
        pseudoDT[0] = rot - spd;
        pseudoDT[1] = spd + rot;
        pseudoDT[2] = spd + rot;
        pseudoDT[3] = rot - spd;
        double max = pseudoDT[0];
        for(int i = 1;i < VICTOR_COUNT; i++) {
            if(max < Math.abs(pseudoDT[i])) max = pseudoDT[i];
        }
        if(max > 1) {
            for(int i = 0; i < VICTOR_COUNT; i++) {
                pseudoDT[i] /= max;
            }
        }
        for(int i = 0; i < VICTOR_COUNT; i++) {
            DT[i].set(pseudoDT[i]);
        }
    }
}